#include "stm32f10x.h"                  // Device header
#include "CarMove.h"
#include "CarPWM.h"
#include "Delay.h"
#include "PID.h"
#include "LED.h"
#include "GWKJ.h"

//#define L2 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_10)
//#define L1 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_11)
//#define M0 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_12)
//#define R1 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)
//#define R2 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)
#define Buzzer_On GPIO_ResetBits(GPIOB,GPIO_Pin_5)
#define Buzzer_Off GPIO_SetBits(GPIOB,GPIO_Pin_5)

int track_error;
int Track_PID_Out,Follow_PID_Out;
unsigned char stop_up,stop_down;
extern unsigned int distance;
extern unsigned char stop_cnt,buzzer_flag,slow_flag,check_flag,hcsr04_flag;
extern int speed,buzzer_time;
extern tPid PidTrack,PidFollow;


/********************************/
extern uint8_t Gray[8];
#define L4 Gray[0]
#define L3 Gray[1]
#define L2 Gray[2]
#define L1 Gray[3]
#define R1 Gray[4]
#define R2 Gray[5]
#define R3 Gray[6]
#define R4 Gray[7]
extern unsigned char stop_ena_flag;
extern uint8_t check_ena_flag;
extern unsigned char mode;

void Track_Init(void)
{
	GW_digital_Init();
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);

	//蜂鸣器初始化
	GPIO_InitTypeDef GPIO_Initstructure;	
	GPIO_Initstructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_Initstructure.GPIO_Pin=GPIO_Pin_5;
	GPIO_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_Initstructure);
	
	GPIO_SetBits(GPIOB,GPIO_Pin_5);
	

	
}

void CarFlag_Check(void)
{	
//	if(mode==0)
//	{
//			if((L3==1&&L2==1&&L1==0&&R1==0&&R2==0&&R3==0)||(L3==1&&L2==0&&L1==0&&R1==0&&R2==0&&R4==1)||(L3==0&&L2==0&&L1==0&&R1==0&&R2==1)){if(check_flag==0)stop_up=1;}//A发车
//			else{stop_up=0;stop_down=1;}
//	    if(stop_up==1&&stop_down==1){if(stop_ena_flag==0){stop_cnt++;if(stop_cnt==1) stop_ena_flag=1;}buzzer_flag=1;stop_down=0;}
//	    if(buzzer_flag==1){Buzzer_On;if(buzzer_time>500)buzzer_flag=0;}
//	    else{buzzer_time=0;Buzzer_Off;}
//	}
//	else
//	{	
			
		if(check_ena_flag==0)
		{	
//			if((L2==0&&L1==0&&R1==0&&R2==1)||(L2==1&&L1==0&&R1==0&&R2==0)||(L2==0&&L1==0&&R1==0&&R2==0)){if(check_flag==0)stop_up=1;}
//			if((L2==0&&L1==0&&R1==0)||(L1==0&&R1==0&&R2==0)||(L1==0&&R1==0&&R2==0)||(L2==0&&L1==0&&R1==0)||(L1==0&&R1==0)|(L2==0&&L1==0&&R1==0&&R2==0)){if(check_flag==0)stop_up=1;}
			if(L1==0&&R1==0){if(check_flag==0)stop_up=1;}
			else{stop_up=0;stop_down=1;}	  
		}				
			if(stop_up==1&&stop_down==1){if(stop_ena_flag==0){if(stop_cnt==1) stop_ena_flag=1;stop_cnt++; check_ena_flag=1;}buzzer_flag=1;stop_down=0;}
			if(buzzer_flag==1){Buzzer_On;if(buzzer_time>500)buzzer_flag=0;}
			else{buzzer_time=0;Buzzer_Off;}
//	}
//	else{stop_up=0;stop_down=1;}
//	if(stop_up==1&&stop_down==1){if(stop_ena_flag==0)stop_cnt++;buzzer_flag=1;stop_down=0;if(stop_cnt==1) stop_ena_flag=1;}
//	if(buzzer_flag==1){Buzzer_On;if(buzzer_time>500)buzzer_flag=0;}
//	else{buzzer_time=0;Buzzer_Off;}
}
//if((L3==1&&L2==1&&L1==0&&R1==0&&R2==0&&R3==0)||(L3==1&&L2==0&&L1==0&&R1==0&&R2==0&&R4==1)){if(check_flag==0)stop_up=1;}//A发车

//(L2==0&&L1==0&&R1==0&&R2==1)||(L2==1&&L1==0&&R1==0&&R2==0)
//外圈(L4==0&&L3==0&&L2==0&&L1==0&&R1==0&&R2==1&&R3==1)||
void CarPid_TrackOut(void)
{
	if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))  //00 1 00  直走
	{
		track_error=0;              //1110 1111       1111 0111
	}
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))    //00 0 10 右转
	{
		track_error=-1;               //1111 1011
	}
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	  //01 0 00 左转
	{
		track_error=1;                 //1101 1111   1011 1111               ||(L4==1&&L3==0&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1)
	}	
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))   //1111 0111//00 1 00 右转
	{
		track_error=-1;       //36       //1110 1111       1111 0111
	}	
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==0&&R4==1))   //1111 0111//00 1 00  右转
	{
		track_error=-1;          //57    //1110 1111       1111 0111
	}	
	else if((L4==1&&L3==0&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))	 //1011 1111//01 0 00 直走
	{
		track_error=0;             //25    //1101 1111   1011 1111               
	}

	LED_OFF();
}

//内圈
void CarPid_TrackIn(void)
{
	if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))  //00 1 00  直走
	{
		track_error=0;              //1110 1111       1111 0111
	}
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))    //00 0 10 右转
	{
		track_error=-1;               //1111 1011
	}
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	  //01 0 00 左转
	{
		track_error=1;                 //1101 1111   1011 1111               ||(L4==1&&L3==0&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1)
	}	
	else if (L3==0)
	{
		if(slow_flag==1)track_error =1;
	}

	LED_ON();
}

//超速
void CarPid_Fast(void)
{
	if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))   //1111 0111//00 1 00  直走
	{
		Car_SetSpeed(speed,speed);
	}
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1)) //1111 1011   //00 0 10 右转
	{
		Car_SetSpeed(-(speed-550),-(speed+250));
	}
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	 //1101 1111//01 0 00 左转
	{
		Car_SetSpeed(-(speed+250),-(speed-550));
	}
	else if((L4==1&&L3==0&&L2==1&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	 //1011 1111//01 0 00 左转
	{
		Car_SetSpeed(-(speed+250),-(speed-550));
	}
	else if((L4==1&&L3==0&&L2==1&&L1==1&&R1==1&&R2==1&&R3==0&&R4==1))	 //1011 1111//01 0 00 直走
	{
		Car_SetSpeed(speed,speed);
	}
	else if((L4==1&&L3==1&&L2==1&&L1==0&&R1==1&&R2==1&&R3==1&&R4==1)) //1111 1011   //00 0 10 右转
	{
		Car_SetSpeed(-(speed-550),-(speed+250));
	}

//	if(L2==0&&L1==0&&M0==1&&R1==0&&R2==0)  //00 1 00  直走
//	{
//		Car_SetSpeed(-speed,-speed);
//	}
//	else if(L2==0&&L1==0&&M0==0&&R1==1&&R2==0)    //00 0 10 右转
//	{
//		Car_SetSpeed(-(speed-550),-(speed+250));
//	}
//	else if(L2==0&&L1==1&&M0==0&&R1==0&&R2==0)		//01 0 00 左转
//	{
//		Car_SetSpeed(-(speed+250),-(speed-550));
//	}
//	
//	else if(L2==0&&L1==1&&M0==1&&R1==0&&R2==0)    //01 1 00 右转
//	{
//		Car_SetSpeed(-(speed-550),-(speed+250));
//	}
//	else if(L2==0&&L1==0&&M0==1&&R1==1&&R2==0)    //00 1 10 右转
//	{
//		Car_SetSpeed(-(speed-550),-(speed+250));
//	}
//	else if(L2==0&&L1==1&&M0==0&&R1==1&&R2==0)    //01 0 10 右转
//	{
//		Car_SetSpeed(-(speed-550),-(speed+250));
//	}
}

void Car_TrackPid(void)
{
	Track_PID_Out=PID_realize(&PidTrack,track_error);
	if(Track_PID_Out>speed)Track_PID_Out=speed;
	if(Track_PID_Out<-speed)Track_PID_Out=-speed;
//	
//	if(hcsr04_flag==1)
//	{
//		if(distance<400)
//		{	
//			Follow_PID_Out=PID_realize(&PidFollow,distance);
//			if(Follow_PID_Out>100)Follow_PID_Out=100;
//			if(Follow_PID_Out<-100)Follow_PID_Out=-100;
//		}
//	else{Follow_PID_Out=40;}
//	}
	
	Car_SetSpeed(speed+Track_PID_Out+Follow_PID_Out,speed-Track_PID_Out+Follow_PID_Out);
}
